#include <ez8.h>

int step_state_left;
int i=0;
void step_left();
void init_timer0();
// Timers:
// Timer 0: Left wheel



#pragma interrupt
void isr_timer0 (void)
{
	step_left();
}

void step_left(void)
{
	// One wonders if this is faster than mod.
	if (step_state_left == 3)
			step_state_left = -1;
	step_state_left++;

	
	switch (step_state_left)
	{
		case 0:
			PBOUT &= 0xE0;
			PBOUT |= 0x10;
			break;
		case 1:
			PBOUT &= 0xE0;
			PBOUT |= 0x04;
			break;
		case 2:
			PBOUT &= 0xE0;
			PBOUT |= 0x02;
			break;
		case 3:
			PBOUT &= 0xE0;
			PBOUT |= 0x08;
			break;
	
	}
}

#pragma interrupt
void isrSW1 (void)
{	
	while(!(PDIN & 0x04));
	i++;
	init_timer0();
}

void InitSW1 (void)
{
	DI();
	PDDD = 0x04;
	SET_VECTOR(P2AD, isrSW1);
	IRQ1ENL = 0x04;
	IRQ1ENH = 0x04;
	IRQES = 0x00;
	IRQPS = 0x04;
	IRQ1 &=~0x04;
	EI();
}

void init_timer0(void) 
{
	DI();
	T0CTL = 0x79;				//0111 1001
	T0H = 0x00; T0L = 0x01;      
	switch(i){
		case 0:
		T0RH = 0x10; T0RL = 0xE0;
		break;
		case 1:
		T0RH = 0x13; T0RL = 0xB0;
		break;
		case 2:
		T0RH = 0x16; T0RL = 0x80;
		break;
		case 3:
		T0RH = 0x19; T0RL = 0x50;
		break;
		case 4:
		T0RH = 0x1C; T0RL = 0x20;
		break;
	}

	SET_VECTOR(TIMER0,isr_timer0);
	IRQ0ENH |= 0x20;             
	IRQ0ENL |= 0x20;
	IRQ0 &= ~0x20;               
	EI();
	//T0CTL |= 0x80;                // Enable timer1
}




void step_init()
{
	step_state_left = 0;

	

	// Set up timers
	init_timer0();

	// Set up ports
	PBDD = 0x00;
	PBHDE = 0xFF;
	PCDD &= ~0x3C;
	PCHDE |= 0x3C;				// 3C = 0011 1010
	PCOUT = 0x3C;
}

int main(void)
{	
	step_init();
	InitSW1();

	T0CTL |= 0x80;                // Enable timer1
	while (1);

	return 0;
}
